
#include <stdlib.h>
#include <memory.h>

#include "pkcs5_padding.h"

unsigned char pkcs5_get_padlen(long block_size, long input_len)
{
	unsigned char padlen = 0;
	long cipherLen = 0;

	//Padding 
	if(input_len % block_size != 0)
	{
		cipherLen = (input_len + block_size - 1) / block_size * block_size ;
	}
	else
	{
		cipherLen = input_len + block_size;
	}

	padlen = (unsigned char)(cipherLen - input_len);

	return padlen;
}

long pkcs5_padding(long block_size, unsigned char * input, long input_len, long * out_len)
{
	unsigned char padlen = 0;
	long cipherLen = 0;
	int i = 0;

	//Padding 
	if(input_len % block_size != 0)
	{
		cipherLen = (input_len + block_size - 1) / block_size * block_size ;
	}
	else
	{
		cipherLen = input_len + block_size;
	}

	padlen = (unsigned char)(cipherLen - input_len);

	* out_len = cipherLen;
	if(input == NULL)
	{
		return 0;
	}

	if( * out_len < cipherLen)
	{
		return 2;
	}

	for(i = 0; i < padlen; i ++)
	{
		input[input_len + i] = padlen;
	}

	return 0;
}

long pkcs5_unpadding(long block_size, unsigned char * input, long input_len, long * out_len)
{
	unsigned char padLen = 0;
	int i = 0;

	if(input_len % block_size != 0)
		return 1;

	padLen = input[input_len - 1];
	if(padLen > block_size)
	{
		//invalid format
		return 2;
	}

	for(i = input_len - 1; i > input_len - padLen; i --)
	{
		if(input[i] != padLen)
		{
			//invalid format
			return 2;
		}
	}

	* out_len = input_len - padLen;
	return 0;
}
